#include "PclMatch.h"

PclMatch::PclMatch(std::shared_ptr<Params>& params_): params(params_)
{
    // 定义nh
    ros::NodeHandle nh;
    // point_cloud_sub 发布数据,绑定回调函数pointCloudCallback
    sub_point_cloud = nh.subscribe<sensor_msgs::PointCloud2>("/scan_matched_points2", 1, &PclMatch::pointCloudCallback, this);

    // 初始化发布者对象
    pub_point_cloud = nh.advertise<sensor_msgs::PointCloud2>("processed_point_cloud", 1);
    pub_voxel = nh.advertise<sensor_msgs::PointCloud2>("voxel_filtered_cloud", 1);
    pub_cross = nh.advertise<sensor_msgs::PointCloud2>("cross_cloud", 1);
    pub_keypoints = nh.advertise<sensor_msgs::PointCloud2>("keypoints_cloud", 1);
    pub_interpolated = nh.advertise<sensor_msgs::PointCloud2>("interpolated_cloud", 1);
    pub_curve = nh.advertise<sensor_msgs::PointCloud2>("curve_cloud", 1);


}
// 通用的发布点云函数
void PclMatch::publishPointCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, ros::Publisher& pub)
{
    sensor_msgs::PointCloud2 output;
    pcl::toROSMsg(*cloud, output);
    output.header.frame_id = "map"; // 设置frame_id
    output.header.stamp = ros::Time::now(); // 设置时间戳
    pub.publish(output);
}
// 将二维点云扩展为三维点云（z坐标设置为0），并添加intensity字段
pcl::PointCloud<pcl::PointXYZI>::Ptr PclMatch::expandPointCloudTo3D(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_with_intensity(new pcl::PointCloud<pcl::PointXYZI>);
    for (const auto& point : cloud->points) {
        pcl::PointXYZI p;
        p.x = point.x;
        p.y = point.y;
        p.z = 0.0; // 设置z坐标为0
        p.intensity = 1.0; // 设置默认的intensity值，可以根据需要进行修改
        cloud_with_intensity->points.push_back(p);
    }
    return cloud_with_intensity;
}

// 检查两个点云间距离大于0.01米，如果大于0.01米，且小于0.1米则连续插入点云，直到点云间距小于0.01米
pcl::PointCloud<pcl::PointXYZI>::Ptr PclMatch::checkAndInsertPoints(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, float min_distance, float max_distance)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_with_interpolated_points(new pcl::PointCloud<pcl::PointXYZI>);
    for (size_t i = 0; i < cloud->points.size() - 1; ++i) {
        const auto& point1 = cloud->points[i];
        const auto& point2 = cloud->points[i + 1];
        float distance = std::sqrt(std::pow(point2.x - point1.x, 2) + std::pow(point2.y - point1.y, 2));
        if (distance > min_distance && distance < max_distance) {
            int num_insertions = static_cast<int>(distance / min_distance);
            float dx = (point2.x - point1.x) / (num_insertions + 1);
            float dy = (point2.y - point1.y) / (num_insertions + 1);
            for (int j = 1; j <= num_insertions; ++j) {
                pcl::PointXYZI p;
                p.x = point1.x + j * dx;
                p.y = point1.y + j * dy;
                p.z = 0.0;
                p.intensity = 1.0;
                cloud_with_interpolated_points->points.push_back(p);
            }
        }
    }
    return cloud_with_interpolated_points;
}

/**
 * @brief 对点云数据进行预处理，包括下采样和去除噪声。
 * 
 * @param cloud_msg 输入的点云数据。
 * @param leaf_size 下采样时的体素大小，默认为0.01。
 * @param mean_k 统计滤波时的邻域点个数，默认为50。
 * @param stddev_mul_thresh 统计滤波时的标准差倍数阈值，默认为1.0。
 * @return pcl::PointCloud<pcl::PointXYZI>::Ptr 预处理后的点云数据。
 */
pcl::PointCloud<pcl::PointXYZI>::Ptr PclMatch::preprocessPointCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_msg, int mean_k, double stddev_mul_thresh )
{
    // 去除噪声
    pcl::StatisticalOutlierRemoval<pcl::PointXYZI> sor;
    sor.setInputCloud(cloud_msg);
    sor.setMeanK(mean_k);
    sor.setStddevMulThresh(stddev_mul_thresh);
    pcl::PointCloud<pcl::PointXYZI>::Ptr sor_filtered_cloud(new pcl::PointCloud<pcl::PointXYZI>);
    sor.filter(*sor_filtered_cloud);

    return sor_filtered_cloud;
}

/**
 * @brief 根据输入的点云拟合线段，并发布所有线段的点云。
 * 1.对points进行分组,将连续的点, 且两点间距不大于0.01, 视作线段，分为一组;
 * 2.对分组后的线段,进行筛选，如果线段长度低于10则抛弃，打印每组线段长度；
 * 
 * @param points 输入的点云，包含若干点。
 * @param line_pub 用于发布线段点云的ROS发布器。
 * @param size_threshold 线段的长度阈值，默认为100。
 * @return std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> 拟合得到的线段集合。
 */
std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> PclMatch::fitLines(const pcl::PointCloud<pcl::PointXYZI>::Ptr& points, ros::Publisher& line_pub, int size_threshold,float min_distance)
{
    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> lines;
    pcl::PointCloud<pcl::PointXYZI>::Ptr current_line(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr all_lines(new pcl::PointCloud<pcl::PointXYZI>); // 用于存储所有线段的点云

    // 打印points的大小
    ROS_INFO("Total points: %lu", points->points.size());
    for (size_t i = 0; i < points->points.size() - 1; ++i) {
        const auto& point1 = points->points[i];
        const auto& point2 = points->points[i + 1];
        float distance = std::sqrt(std::pow(point2.x - point1.x, 2) + std::pow(point2.y - point1.y, 2));
        // 打印distance的大小
        // ROS_INFO("Distance: %f", distance);
        if (distance <= min_distance) { // 0.02
            current_line->points.push_back(point1);
        } else {
            if (current_line->points.size() >= size_threshold) {
                current_line->points.push_back(point1);
                lines.push_back(current_line);
                *all_lines += *current_line; // 将当前线段添加到所有线段的点云中
                ROS_INFO("Line segment length: %lu", current_line->points.size());
            }
            current_line.reset(new pcl::PointCloud<pcl::PointXYZI>);
        }
    }
    // 添加最后一个线段
    if (current_line->points.size() >= size_threshold) {
        current_line->points.push_back(points->points.back());
        lines.push_back(current_line);
        *all_lines += *current_line; // 将当前线段添加到所有线段的点云中
        ROS_INFO("Line segment length: %lu", current_line->points.size());
    }

    // 打印lines的大小
    ROS_INFO("Total lines: %lu", lines.size());

    // 发布所有线段的点云
    publishPointCloud(all_lines, line_pub);
    return lines;
}

// 检查线段组中是否存在直角
Eigen::Vector3f PclMatch::checkRightAngleInLines(const std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& lines, ros::Publisher& angle_pub, size_t step, float min_angle)
{
    static Eigen::Vector3f last_intersection_point = Eigen::Vector3f::Zero();

    for (const auto& line : lines) {
        if (line->points.size() < step * 2 + 1) {
            ROS_WARN("Not enough points in a line to check for right angles.");
            continue;
        }
        for (size_t i = step; i < line->points.size() - step; ++i) {
            Eigen::Vector3f p1(line->points[i - step].x, line->points[i - step].y, line->points[i - step].z);
            Eigen::Vector3f p2(line->points[i].x, line->points[i].y, line->points[i].z);
            Eigen::Vector3f p3(line->points[i + step].x, line->points[i + step].y, line->points[i + step].z);

            Eigen::Vector3f dir1 = p2 - p1;
            Eigen::Vector3f dir2 = p3 - p2;

            float dot_product = dir1.dot(dir2);
            float magnitude1 = dir1.norm();
            float magnitude2 = dir2.norm();
            float angle = std::acos(dot_product / (magnitude1 * magnitude2));

            // 检查是否为直角（90度）
            if (std::abs(angle - M_PI / 2) < min_angle) { // 0.2
                // 打印检测到的直角交点坐标
                ROS_INFO("Right angle found at point: (%f, %f, %f)", p2.x(), p2.y(), p2.z());

                // 计算当前交点与上次交点的坐标偏差
                if (last_intersection_point != Eigen::Vector3f::Zero()) {
                    Eigen::Vector3f deviation = p2 - last_intersection_point;
                    ROS_INFO("Deviation from last intersection point: (%f, %f)", deviation.x(), deviation.y());
                    // 打印两点间的距离
                    ROS_INFO("Distance between 2 p: %f", deviation.norm());
                    // 如果距离大于0.03米，则打印警告信息
                    if (deviation.norm() > 0.03) {
                        ROS_ERROR("Distance between two intersection points is greater than 0.03 meters.");
                    }
                }
                // 更新上次交点坐标
                last_intersection_point = p2;

                // 发布检测到直角时的p1, p2, p3
                pcl::PointCloud<pcl::PointXYZI>::Ptr angle_points(new pcl::PointCloud<pcl::PointXYZI>);
                pcl::PointXYZI point1, point2, point3;
                point1.x = p1.x(); point1.y = p1.y(); point1.z = p1.z(); point1.intensity = 1.0;
                point2.x = p2.x(); point2.y = p2.y(); point2.z = p2.z(); point2.intensity = 1.0;
                point3.x = p3.x(); point3.y = p3.y(); point3.z = p3.z(); point3.intensity = 1.0;
                angle_points->points.push_back(point1);
                angle_points->points.push_back(point2);
                angle_points->points.push_back(point3);
                publishPointCloud(angle_points, angle_pub);

                return p2;
            }
        }
    }

    ROS_INFO("No right angles found in any of the lines.");
    return last_intersection_point;
}

// 回调函数，用于处理接收到的点云数据
void PclMatch::pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
    //发布原始数据
    pub_point_cloud.publish(cloud_msg);

    // 将ROS的点云消息转换为PCL的点云格式
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*cloud_msg, *cloud);

    // 将二维点云扩展为三维点云（z坐标设置为0），并添加intensity字段
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_with_intensity 
                                    = expandPointCloudTo3D(cloud);

    //同时增加点云密度
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_with_interpolated_points 
                                    = checkAndInsertPoints(cloud_with_intensity, 0.01, 0.10);
    // 发布插值后的点云
    publishPointCloud(cloud_with_interpolated_points, pub_interpolated);

    // 数据预处理
    pcl::PointCloud<pcl::PointXYZI>::Ptr preprocessed_cloud 
                                    = preprocessPointCloud(cloud_with_interpolated_points,3, 1.0);
    // 发布预处理后的点云
    publishPointCloud(preprocessed_cloud, pub_voxel);


    // 直线拟合
    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> lines 
                                    = fitLines(preprocessed_cloud, pub_curve);
    
    // 检查lines是否为空
    if (!lines.empty()) {
        // 检查直角
        Eigen::Vector3f cross_point = checkRightAngleInLines(lines, pub_keypoints);
        params->g_cross_point_.x = cross_point.x()-0.5;
        params->g_cross_point_.y = cross_point.y()-0.5;
        params->g_cross_point_.yaw = 3.14;
        // 设置新的目标点，其坐标是cross_point的坐标x-0.5,y-0.5
        pcl::PointXYZI target_point;
        target_point.x = cross_point.x() - 0.5;
        target_point.y = cross_point.y() - 0.5;
        target_point.z = 0.0;
        target_point.intensity = 1.0;
        // 发布目标点
        pcl::PointCloud<pcl::PointXYZI>::Ptr target_points(new pcl::PointCloud<pcl::PointXYZI>);
        target_points->points.push_back(target_point);
        publishPointCloud(target_points, pub_cross);

        // 发布lines中第一条线段的点云
        // publishPointCloud(lines[0], keypoints_pub);
    } else {
        ROS_WARN("No lines were fitted.");
    }

}
